#pragma once

#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <fstream> 
#include <vector>

#include "common/config.h"
#include "common/data.h"

class CubicPolynomial {
public:
    CubicPolynomial(const PathPoint& start_path_point, const PathPoint& end_path_point);
    inline float max_s() const { return end_path_point_.s; }
    inline float min_s() const { return start_path_point_.s; }
    inline const CubicPolynomialCoeffs& coeffs() const { return coeffs_; }
    inline float get_l_by_s(float s) const { return coeffs_.a0 + coeffs_.a1 * s + coeffs_.a2 * s * s + coeffs_.a3 * s * s * s; }
    static Eigen::Vector4f compute_cubic_polynomial_coeffs(const PathPoint& p1, const PathPoint& p2);

private:
    PathPoint start_path_point_;
    PathPoint end_path_point_;
    CubicPolynomialCoeffs coeffs_;
};

class Path {
public:
    Path(int path_index, float segment_length, const std::vector<RefPoint>* refline, const std::vector<PathPoint>& key_path_points);

    float get_l_by_s(float s) const ;
    void get_point_info_by_s(float s, PointInfo* point_info) const;
    inline float length() const { return max_s_ - min_s_; }

    int serialize_path_info(PathInfo* buffer) const;  // 将path的信息序列化, 方便GPU使用
    int serialize_path_coeffs(CubicPolynomialCoeffs* buffer) const;
    void sampling_on_cpu(float delat_s) const;
    void dump(std::ofstream& out_file, float delat_s) const;

private:
    int path_index_;
    float segment_length_ = 0.0;
    float min_s_ = 0.0;
    float max_s_ = 0.0;

    const std::vector<RefPoint>* refline_ = nullptr;
    PathInfo path_info_;

    std::vector<CubicPolynomial> cubic_polynomial_vec_;
};